#!/usr/bin/env python3
# -*- coding: utf-8 -*-

"""
简化的舵机控制器
Simplified Servo Controller

为LLM项目提供简化的舵机控制功能，去除复杂依赖
"""

import serial
import time
import logging
import threading
import math
from typing import Optional, Tuple, Dict, Any

try:
    from .servo_protocol import ServoProtocol
except ImportError:
    from servo_protocol import ServoProtocol

logger = logging.getLogger(__name__)


class SimpleServoController:
    """
    简化的舵机控制器类
    """
    
    def __init__(self, port='/dev/ttyS1', baudrate=115200, 
                 pan_pin=3, tilt_pin=4, init_pan_angle=135.0, init_tilt_angle=135.0):
        """
        初始化舵机控制器
        
        Args:
            port (str): 串口设备名
            baudrate (int): 波特率
            pan_pin (int): 水平舵机引脚
            tilt_pin (int): 垂直舵机引脚
            init_pan_angle (float): 水平初始角度
            init_tilt_angle (float): 垂直初始角度
        """
        self.port = port
        self.baudrate = baudrate
        self.pan_pin = pan_pin
        self.tilt_pin = tilt_pin
        self.serial = None
        self.connected = False
        self.lock = threading.Lock()
        
        # 当前舵机角度
        self.current_pan = init_pan_angle
        self.current_tilt = init_tilt_angle
        
        # 初始化协议处理器
        self.protocol = ServoProtocol()
        
        # 控制统计
        self.control_stats = {
            'total_commands': 0,
            'successful_commands': 0,
            'failed_commands': 0,
            'last_command_time': 0.0,
            'last_pan_angle': init_pan_angle,
            'last_tilt_angle': init_tilt_angle
        }
        
        logger.info(f"简化的舵机控制器初始化完成")
        logger.info(f"串口: {port}, 波特率: {baudrate}")
        logger.info(f"引脚: Pan={pan_pin}, Tilt={tilt_pin}")
        logger.info(f"初始角度: Pan={init_pan_angle}°, Tilt={init_tilt_angle}°")
    
    def connect(self) -> bool:
        """
        连接到串口设备
        
        Returns:
            bool: 是否成功连接
        """
        try:
            logger.info(f"尝试连接串口: {self.port}, 波特率: {self.baudrate}")
            self.serial = serial.Serial(self.port, self.baudrate, timeout=1)
            self.connected = True
            logger.info("串口连接成功")
            return True
        except Exception as e:
            logger.error(f"串口连接失败: {e}")
            self.connected = False
            return False
    
    def disconnect(self):
        """断开串口连接"""
        if self.serial and self.serial.is_open:
            try:
                self.serial.close()
                self.connected = False
                logger.info("串口已断开连接")
            except Exception as e:
                logger.error(f"串口断开连接异常: {e}")
    
    def move_servos(self, pin: int, angle: float) -> bool:
        """
        移动指定引脚的舵机到指定角度
        
        Args:
            pin (int): 舵机引脚
            angle (float): 目标角度
            
        Returns:
            bool: 是否成功执行
        """
        if not self.connected or not self.serial:
            logger.warning("串口未连接")
            return False
        
        # 角度范围检查
        if not (0 <= angle <= 270):
            logger.error(f"角度超出范围: {angle}° (范围: 0-270°)")
            return False
        
        with self.lock:
            try:
                start_time = time.time()
                
                # 生成协议数据
                servo_data = {pin: angle}
                protocol_data = self.protocol.generate_protocol(servo_data)
                
                # 发送数据
                self.serial.write(protocol_data)
                self.serial.flush()
                
                # 更新当前角度
                if pin == self.pan_pin:
                    self.current_pan = angle
                elif pin == self.tilt_pin:
                    self.current_tilt = angle
                
                # 更新统计
                execution_time = time.time() - start_time
                self._update_stats(True, execution_time)
                
                logger.debug(f"舵机移动成功: Pin={pin}, Angle={angle}°, 耗时={execution_time:.3f}s")
                return True
                
            except Exception as e:
                logger.error(f"舵机移动异常: {e}")
                self._update_stats(False, time.time() - start_time)
                return False
    
    def move_to_angle(self, pan: float, tilt: float) -> bool:
        """
        同时移动水平和垂直舵机到指定角度
        
        Args:
            pan (float): 水平角度 (0-270度)
            tilt (float): 垂直角度 (0-270度)
            
        Returns:
            bool: 是否成功执行
        """
        if not self.connected or not self.serial:
            logger.warning("串口未连接")
            return False
        
        # 角度范围检查
        if not (0 <= pan <= 270):
            logger.error(f"水平角度超出范围: {pan}° (范围: 0-270°)")
            return False
        
        if not (0 <= tilt <= 270):
            logger.error(f"垂直角度超出范围: {tilt}° (范围: 0-270°)")
            return False
        
        with self.lock:
            try:
                start_time = time.time()
                
                # 针对水平舵机的硬件安装方向进行修正
                # 由于水平舵机安装方向导致角度-方向映射反向，需要修正
                corrected_pan = 270.0 - pan  # 反转水平角度以匹配直觉方向
                
                # 生成协议数据（同时控制两个舵机）
                servo_data = {
                    self.pan_pin: corrected_pan,  # 使用修正后的水平角度
                    self.tilt_pin: tilt  # 垂直角度不需要修正
                }
                protocol_data = self.protocol.generate_protocol(servo_data)
                
                # 发送数据
                self.serial.write(protocol_data)
                self.serial.flush()
                
                # 更新当前角度（保存逻辑角度，便于后续计算）
                self.current_pan = pan
                self.current_tilt = tilt
                
                # 更新统计
                execution_time = time.time() - start_time
                self._update_stats(True, execution_time)
                
                logger.debug(f"云台移动成功: Pan={pan}°(硬件:{corrected_pan:.1f}°), Tilt={tilt}°, 耗时={execution_time:.3f}s")
                return True
                
            except Exception as e:
                logger.error(f"云台移动异常: {e}")
                self._update_stats(False, time.time() - start_time)
                return False
    
    def move_relative(self, pan_delta: float, tilt_delta: float) -> bool:
        """
        相对移动云台
        
        Args:
            pan_delta (float): 水平角度变化量
            tilt_delta (float): 垂直角度变化量
            
        Returns:
            bool: 是否成功执行
        """
        # 水平方向：正值表示向右，负值表示向左（已修复方向反转问题）
        new_pan = self.current_pan + pan_delta
        new_tilt = self.current_tilt + tilt_delta
        
        # 限制角度范围
        new_pan = max(0, min(270, new_pan))
        new_tilt = max(0, min(270, new_tilt))
        
        return self.move_to_angle(new_pan, new_tilt)
    
    def reset_position(self) -> bool:
        """
        重置云台到初始位置
        
        Returns:
            bool: 是否成功执行
        """
        return self.move_to_angle(135.0, 135.0)
    
    def get_current_angles(self) -> Tuple[float, float]:
        """
        获取当前角度
        
        Returns:
            Tuple[float, float]: (水平角度, 垂直角度)
        """
        return self.current_pan, self.current_tilt
    
    def get_status(self) -> Dict[str, Any]:
        """
        获取控制器状态
        
        Returns:
            Dict[str, Any]: 状态信息
        """
        return {
            'connected': self.connected,
            'current_angles': {
                'pan': self.current_pan,
                'tilt': self.current_tilt
            },
            'config': {
                'serial_port': self.port,
                'baudrate': self.baudrate,
                'pan_pin': self.pan_pin,
                'tilt_pin': self.tilt_pin
            },
            'stats': self.control_stats.copy()
        }
    
    def _update_stats(self, success: bool, execution_time: float):
        """更新控制统计"""
        self.control_stats['total_commands'] += 1
        if success:
            self.control_stats['successful_commands'] += 1
        else:
            self.control_stats['failed_commands'] += 1
        
        self.control_stats['last_command_time'] = execution_time
        self.control_stats['last_pan_angle'] = self.current_pan
        self.control_stats['last_tilt_angle'] = self.current_tilt